COMMENT ⊗   VALID 00013 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00003 00002	BEGIN "LS"
C00005 00003	SUBR ROTATE(REFERENCE REAL X,Y REAL Q)
C00006 00004	SUBR MKCAMR			α COSINES BETWEEN CAMERA RAYS
C00008 00005	SUBR MKFORK		α INITIAL DIRECTION COSINES OF CAMERA RAYS
C00010 00006	SUBR SETFORK (REAL W1,W2,W3)
C00012 00007	SUBR SKEW (REAL MX,MY,MZ,NX,NY,NZ)
C00013 00008	SUBR MKFORCE		α FIND CLOSEST POINTS OF CORRESPONDING SKEW RAYS
C00014 00009	SUBR MKTORQ(REAL W1,W2,W3)	α TORQUE VECTORS
C00015 00010	REAL SUBR QQ(REAL W1,W2,W3)
C00016 00011	SUBR TORQUE
C00020 00012	SUBR CLIMB
C00022 00013	BEGIN "MAIN"				α MAIN EXECUTION
C00023 ENDMK
C⊗;
BEGIN "LS"
	REQUIRE "ABBREV[SYS,BGB]" SOURCE_FILE;

α SOLUTION TO TEST CASE;
	REAL C1X,C1Y,C1Z;
	REAL C2X,C2Y,C2Z;
	REAL WW1,WW2,WW3;

α TWO CAMERA FORKS;
	REAL SS1,SS2,RR1,RR2;			α CAMERA CIRCLE CENTERS & RADII;
	REAL α1,α2,MS12,NS12;			α ANGLE BETWEEN FIRST TWO RAYS;
	REAL MC12,MC13,MC14,MC23,MC24,MC34;	α COSINES OF CAMERA RAYS;
	REAL NC12,NC13,NC14,NC23,NC24,NC34;
	REAL MSIGN3,MSIGN4,NSIGN3,NSIGN4;

α POSITION DIRECTION COSINES ACCORDING TO W1,W2,W3;
	REAL M3X,M3Y,M3Z;	REAL M4X,M4Y,M4Z;
	REAL N3X,N3Y,N3Z;	REAL N4X,N4Y,N4Z;
	REAL UX,UY,UZ;		REAL U3X,U3Y,U3Z;	REAL U4X,U4Y,U4Z;
	REAL VX,VY,VZ;		REAL V3X,V3Y,V3Z;	REAL V4X,V4Y,V4Z;

α INITIAL CAMERA RAY DIRECTION COSINES;
	REAL M30X,M30Y,M30Z;
	REAL M40X,M40Y,M40Z;
	REAL N30X,N30Y,N30Z;
	REAL N40X,N40Y,N40Z;

α PARAMETERS OF CLOSEST APPROACH BETWEEN CORRESPONDING SKEW RAYS;
	REAL P,Q;
	REAL F3X,F3Y,F3Z,D3;
	REAL F4X,F4Y,F4Z,D4;

α TORQUE ANGLES (OF DELTA OMEGA);
	REAL DW0,DW1,DW2,DW3;

SUBR ROTATE(REFERENCE REAL X,Y; REAL Q);
BEGIN	REAL C,S,T;
	C ← COS(Q); S ← SIN(Q);
	T ← C*X - S*Y;
	Y ← C*Y + S*X;
	X ← T;
END;
SUBR MKCAMR;			α COSINES BETWEEN CAMERA RAYS;
BEGIN "MKCAMR"

	MC12 ← .8630286; NC12 ←.6801704;	α TEST COSINES;
	MC13 ← .9280318; NC13 ←.8939879;
	MC14 ← .9504978; NC14 ←.8864540;
	MC23 ← .9674100; NC23 ←.9180645;
	MC24 ← .9387822; NC24 ←.8159213;
	MC34 ← .9240094; NC34 ←.8619705;

	MSIGN3 ← -1;	MSIGN4 ← +1;		α TEST SIGNS;
	NSIGN3 ← -1;	NSIGN4 ← +1;

	α1 ← ACOS(MC12); MS12 ← SIN(α1);
	α2 ← ACOS(NC12); NS12 ← SIN(α2);

	SS1 ← MC12/MS12; RR1 ← 1/MS12;
	SS2 ← NC12/NS12; RR2 ← 1/NS12;
	
α TEST SOLUTION;
	C1X ← 3.628192;	C1Y ← -.4830575; C1Z ← 0;
	C2X ← 2.211233; C2Y ←  .2988597; C2Z ←.4614799;

	WW1 ← ATAN2(C1Y,C1X-SS1);
	WW2 ← ASIN(C2Y/RR2);
	WW3 ← ATAN2(-C2Z,C2X);
END "MKCAMR";
SUBR MKFORK;		α INITIAL DIRECTION COSINES OF CAMERA RAYS;
BEGIN "MKFORK"
	M30X ← -MC13;
	M30Y ←-(MC23-MC12*MC13)/MS12;
	M30Z ← MSIGN3*SQRT(1-M30X↑2-M30Y↑2);

	M40X ← -MC14;
	M40Y ←-(MC24-MC12*MC14)/MS12;
	M40Z ← MSIGN4*SQRT(1-M40X↑2-M40Y↑2);

	N30X ← -NC13;
	N30Y ←-(NC23-NC12*NC13)/NS12;
	N30Z ← NSIGN3*SQRT(1-N30X↑2-N30Y↑2);

	N40X ← -NC14;
	N40Y ←-(NC24-NC12*NC14)/NS12;
	N40Z ← NSIGN4*SQRT(1-N40X↑2-N40Y↑2);
END "MKFORK";
SUBR SETFORK (REAL W1,W2,W3);
BEGIN "SETFORK"
	REAL OM1,OM2;

α COPY THE INITIAL FORK VECTORS;
	M3X ← M30X;	M3Y ← M30Y;	M3Z ← M30Z;
	M4X ← M40X;	M4Y ← M40Y;	M4Z ← M40Z;
	N3X ← N30X;	N3Y ← N30Y;	N3Z ← N30Z;
	N4X ← N40X;	N4Y ← N40Y;	N4Z ← N40Z;

α FORK ORIENTATION ANGLES;
	OM1 ← ATAN2(1-RR1*SIN(W1),SS1+RR1*COS(W1));
	OM2 ← ATAN2(1-RR2*SIN(W2),SS2+RR2*COS(W2));
	ROTATE(M3X,M3Y,-OM1);	ROTATE(M4X,M4Y,-OM1);
	ROTATE(N3X,N3Y,-OM2);	ROTATE(N4X,N4Y,-OM2);
	ROTATE(N3Z,N3X,W3);	ROTATE(N4Z,N4X,W3);

α CAMERA LOCII;
	UX ←  SS1 + RR1*COS(W1);
	UY ←        RR1*SIN(W1);
	UZ ←  0;

	VX ← COS(W3)*(SS2 + RR2*COS(W2));
	VY ←                RR2*SIN(W2) ;
	VZ ←-SIN(W3)*(SS2 + RR2*COS(W2));
END "SETFORK";
SUBR SKEW (REAL MX,MY,MZ,NX,NY,NZ);
BEGIN "SKEW"
	REAL MN;
	MN ← MX*NX + MY*NY + MZ*NZ;

	P ←	((UX-VX)*(NX*MN-MX)
		+(UY-VY)*(NY*MN-MY)
		+(UZ-VZ)*(NZ*MN-MZ)) / (1-MN↑2);

	Q ←	((UX-VX)*(NX-MN*MX)
		+(UY-VY)*(NY-MN*MY)
		+(UZ-VZ)*(NZ-MN*MZ)) / (1-MN↑2);
END "SKEW";
SUBR MKFORCE;		α FIND CLOSEST POINTS OF CORRESPONDING SKEW RAYS;
BEGIN "MKFORCE"
	SKEW(M3X,M3Y,M3Z,N3X,N3Y,N3Z);
	U3X ← UX + P*M3X;	V3X ← VX + Q*N3X;
	U3Y ← UY + P*M3Y;	V3Y ← VY + Q*N3Y;
	U3Z ← UZ + P*M3Z;	V3Z ← VZ + Q*N3Z;

	SKEW(M4X,M4Y,M4Z,N4X,N4Y,N4Z);
	U4X ← UX + P*M4X;	V4X ← VX + Q*N4X;
	U4Y ← UY + P*M4Y;	V4Y ← VY + Q*N4Y;
	U4Z ← UZ + P*M4Z;	V4Z ← VZ + Q*N4Z;

α FORCE VECTORS;
	F3X ← U3X-V3X;	F3Y ← U3Y-V3Y;	F3Z ← U3Z-V3Z;
	F4X ← U4X-V4X;	F4Y ← U4Y-V4Y;	F4Z ← U4Z-V4Z;

α SEPARATION DISTANCE;
	D3 ← SQRT(F3X↑2+F3Y↑2+F3Z↑2);
	D4 ← SQRT(F4X↑2+F4Y↑2+F4Z↑2);
END "MKFORCE";
SUBR MKTORQ(REAL W1,W2,W3);	α TORQUE VECTORS;
BEGIN "MKTORQ"
	DW1 ← -F3X*U3Y + F3Y*(U3X-SS1)
	      -F4X*U4Y + F4Y*(U4X-SS1);

	DW2 ←	-SIN(W3)*
			(+ F3Y*(V3Z+SS2*SIN(W3)) - F3Z*V3Y 
			 + F4Y*(V4Z+SS2*SIN(W3)) - F4Z*V4Y)
		+COS(W3)*
			(- F3X*(V3Z+SS2*SIN(W3)) + F3Z*(V3X-SS2*COS(W3))
			 - F4X*(V4Z+SS2*SIN(W3)) + F4Z*(V4X-SS2*COS(W3)) );

	DW3 ← + F3X*V3Z - F3Z*V3X + F4X*V4Z - F4Z*V4X;

	DW0 ← SQRT(DW1↑2 + DW2↑2 + DW3↑2);
	DW1 ← DW1/DW0;
	DW2 ← DW2/DW0;
	DW3 ← DW3/DW0;
END "MKTORQ";
REAL SUBR QQ(REAL W1,W2,W3);
BEGIN "QQ"
	SETFORK(W1,W2,W3);
	MKFORCE;
	RETURN(1/(1+D3+D4));
END "QQ";

SUBR TORQUE;
BEGIN "TORQUE"
	REAL Q00,Q0,W1,W2,W3;
	ITG I,J;
	REAL Q1,Q2,Q3,K;
	STRING STR; ITG CHR;

	OUTSTR(↓&↓&"TORQUE CLIMBER."&↓);
	OUTSTR("INITIAL GUESS ? ");
	STR←INCHWL;
	W1←REALSCAN(STR,CHR)*π/180;
	W2←REALSCAN(STR,CHR)*π/180;
	W3←REALSCAN(STR,CHR)*π/180;
	K ←REALSCAN(STR,CHR);
	IF K=0 THEN K←0.01;
	Q0 ← 1.0; 

FOR I←1 THRU 100 DO
BEGIN
	Q00 ← Q0;
	Q0 ← QQ(W1,W2,W3); 
	MKTORQ(W1,W2,W3);
	W1 ← W1 + K*DW1;
	W2 ← W2 + K*DW2;
	W3 ← W3 + K*DW3;
	OUTSTR(CVS(I)&" "&CVG(Q0)&↓);
	IF ABS(Q0-Q00)≤1@-5 THEN DONE;
END;
OUTSTR("W ANGLE'S:	"&CVG(180*W1/π)&" "&CVG(180*W2/π)&" "&CVG(180*W3/π)&↓);
INCHRW;
END "TORQUE";
SUBR CLIMB;
BEGIN "CLIMB"
	REAL W1,W2,W3;	STRING STR;ITG CHR;REAL Q0,Q1,Q2,Q3,Q4,Q00;
	REAL P0,P1,P2,P3,P4;REAL D1,D2;ITG I,J;

OUTSTR("INITIAL GUESS ? ");
	STR←INCHWL;
	W1←REALSCAN(STR,CHR)*π/180;
	W2←REALSCAN(STR,CHR)*π/180;
	W3←REALSCAN(STR,CHR)*π/180;
	
	D1 ← 1@-5;
	D2 ← 1@3;
	Q0 ← QQ(W1,W2,W3);

FOR I←0 THRU 300 DO
BEGIN
	Q1 ← QQ(W1+D1,W2,W3)-Q0;
	Q2 ← QQ(W1,W2+D1,W3)-Q0;
	Q3 ← QQ(W1,W2,W3+D1)-Q0;

α SHORTEN THE STEP SIZE;
	WHILE QQ(W1+D2*Q1,W2+D2*Q2,W3+D2*Q3)<Q0 DO
	IF D2=0.1 THEN DONE ELSE D2 ← (D2*0.80 MAX 0.1);

	IF D2=0.1 THEN 
	IF D1=1@-5 THEN ⊂ D1←1@-6; D2←1.0;⊃ ELSE
	IF D1=1@-6 THEN ⊂ D1←1@-7; D2←1.0;⊃ ELSE DONE;

α LENGTHEN THE STEP SIZE;
	IF D2<300 ∧ D2>0.1 THEN
	WHILE Q0<(Q00←QQ(W1+1.2*D2*Q1,W2+1.2*D2*Q2,W3+1.2*D2*Q3)) DO 
	⊂ Q0←Q00;D2←D2*1.2;⊃;
	Q0←QQ(W1←W1+D2*Q1,W2←W2+D2*Q2,W3←W3+D2*Q3);
END;
	OUTSTR(CVS(I)&" "&CVG(Q0)&CVG(D2)&↓);
OUTSTR("W ANGLE'S:	"&CVG(180*W1/π)&" "&CVG(180*W2/π)&" "&CVG(180*W3/π)&↓);
END "CLIMB";
BEGIN "MAIN"				α MAIN EXECUTION;
	REAL Q0,W1,W2,W3;
	ITG I,J,K;

	MKCAMR;
	MKFORK;
	Q0 ← QQ(WW1,WW2,WW3);
	WHILE TRUE DO CLIMB;
	WHILE TRUE DO ⊂ TORQUE;CLIMB;⊃;

END "MAIN";

END "LS";